#include <string.h> // for memset()
#include <unistd.h> // for usleep()
#include <time.h>   // for nanosleep()

#include "WupperUpload.h"
#include "wuppercard/WupperException.h"
//#include "EmuDataGenerator.h"
//#include "crc.h"

//#define USE_LIB_CMEM

#ifndef USE_LIB_CMEM
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/mman.h>
#endif

#include <iostream>
#include <iomanip>
#include <sstream> // for ostringstream
#include <fstream> // for ifstream
using namespace std;

// NOTE: Ignore the deprecated-declarations warning of gcc13, see Wupper-2310
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
//#include "yaml-cpp/yaml.h"
#pragma GCC diagnostic pop

// ----------------------------------------------------------------------------

WupperUpload::WupperUpload( int card_nr,
                      i64 buffer_size,
                      int dma_index)
  : _wupper( 0 ),
    _cardNr( card_nr ),
    _blockSize( -1 ),
    _bar2( 0 ),
    _toWupperBlockBytes( 32 ),
    _toWupperPayloadBytes( 30 ),
    _bufferSize( 0 ),
    _dataBuffer( 0 ),
    _cmemHandle( -1 ),
    _cmemStartAddr( 0 ),
    _cmemEndAddr( 0 ),
    _dmaIndex( dma_index ),
    _dmaCircular( false ),
    _dmaTlp( 0 ),
    _dmaSoftwPtr( 0 ),
    _bar0( 0 ),
    _dmaDesc( 0 ),
    _dmaStat( 0 ),
    _bytesToUpload( 0 ),
    //_uploadDmaSize( 8*1024 ), // Do not exceed 16KB -> data corruption
    // Corruption at > 1024 (10 Oct 2017)
    //_uploadDmaSize( 1024 ),
    _uploadDmaSize( 0 ), // 'Unlimited' (12 Apr 2022)
    _uploadIndex( 0 ),
    _writeIndex( 0 ),
    _timestamp( 0 ),
    _fillerByte( 0 ),
    _pbChunkCntr( 0 ),
    _pbBlockCntr( 0 ),
    _pbRandomData( false )
{
  // A buffer size of 0 also selects the default size
  if( buffer_size == 0 ) buffer_size = DFLT_SEND_BUF_SIZE;

  // Allocate a (continuous) buffer of the requested size
  _dataBuffer = this->cmemAllocate( buffer_size, card_nr );

  if( _dataBuffer )
    {
      // Initialize the buffer
      memset( static_cast<void *> (_dataBuffer), 0xFF, buffer_size );
      _bufferSize = buffer_size;
    }
  else
    {
      if( !_errString.empty() ) _errString += "; ";
      _errString += "Failed to allocate requested buffer size";
    }

  // Open FELIX Wupper-card
  _wupper = new WupperCard;
  try {
    uint32_t lockbits = LOCK_NONE;
    // Backwards compatibility
    if( dma_index == 0 )
      lockbits |= LOCK_DMA0;
    else if( dma_index == 1 )
      lockbits |= LOCK_DMA1;
    // New lockbits
    if( dma_index > -1 )
      lockbits |= ((1 << dma_index) << 16);

    _wupper->card_open( _cardNr, lockbits );

    // Find DMA index to use?
    if( dma_index == -1 )
      {
        // DMA controller index for FromHost data
        dma_index = _wupper->cfg_get_option(BF_GENERIC_CONSTANTS_DESCRIPTORS) - 1;
        lockbits |= ((1 << dma_index) << 16); // New lockbit
        // Backwards compatibility
        if( dma_index == 0 )
          lockbits |= LOCK_DMA0;
        else if( dma_index == 1 )
          lockbits |= LOCK_DMA1;

        // Reopen device with correct lockbits
        _wupper->card_close();
        _wupper->card_open( _cardNr, lockbits );
      }
    _dmaIndex = dma_index;
  }
  catch( WupperException &ex ) {
    if( !_errString.empty() ) _errString += "; ";
    _errString += "WupperCard open: ";
    _errString += ex.what();
    delete _wupper;
    _wupper = 0;
    return;
  }

  // Get a pointer to the BAR2 registers
  _bar2 = (wuppercard_bar2_regs_t *) _wupper->bar2Address();

  // Get a pointer to the BAR0 registers
  u64 bar0_addr = _wupper->bar0Address();
  _bar0 = (volatile wuppercard_bar0_regs_t *) bar0_addr;
  _dmaDesc = &_bar0->DMA_DESC[dma_index];
  _dmaStat = &_bar0->DMA_DESC_STATUS[dma_index];

  this->dmaStop();
  // Force circular DMA inactive state initially
  // to prevent hanging in upload(): dmaActive()
  _dmaSoftwPtr = _dmaStat->current_address;

  // Get this firmware's to-host data blocksize and trailer format
  _blockSize = 1024;
  _blockSizeCode  = (_blockSize/1024-1) << 8;
  

  _minimumUsleep = this->minimumUsleep();
}

// ----------------------------------------------------------------------------

WupperUpload::~WupperUpload()
{
  if( _pbFile.is_open() ) _pbFile.close();

  // Close the Wupper-card
  try {
    if( _wupper )
      _wupper->card_close();
  }
  catch( WupperException &ex ) {
    // Do something..?
  }
  delete _wupper;

  // Free the receive buffer and close CMEM(_RCC)
#ifdef USE_LIB_CMEM
  int ret = CMEM_SegmentFree( _cmemHandle );
  if( ret == CMEM_RCC_SUCCESS )
    ret = CMEM_Close();
  if( ret != CMEM_RCC_SUCCESS )
    rcc_error_print( stdout, ret );

#else
  if( _dataBuffer )
    {
      munmap( (void *) _cmemDescriptor.uaddr, _cmemDescriptor.size );
      ioctl( _cmemHandle, CMEM_RCC_FREE, &_cmemDescriptor.handle );
      close( _cmemHandle );
    }
#endif // USE_LIB_CMEM
}

// ----------------------------------------------------------------------------

void WupperUpload::stop()
{
  this->dmaStop();
}

// ----------------------------------------------------------------------------

std::string WupperUpload::errorString()
{
  if( _errString.empty() )
    return std::string( "" );

  std::ostringstream oss;
  oss << "WupperUpload#" << _cardNr << ": " << _errString;

  // Clear the error string
  _errString.clear();

  return oss.str();
}

// ----------------------------------------------------------------------------

bool WupperUpload::hasError()
{
  return !_errString.empty();
}

// ----------------------------------------------------------------------------

std::string WupperUpload::debugString()
{
  std::ostringstream oss;
  oss << "WupperUpload" << _cardNr << " DEBUG: "
      << std::hex << setfill('0')
      << "start=" << std::setw(10) << _cmemStartAddr
      << ", end=" << std::setw(10) << _cmemEndAddr
      << ", psoftw=" << std::setw(10) << _dmaDesc->read_ptr
      << ", pfirmw=" << std::setw(10) << _dmaStat->current_address
      << ", even-PC/DMA " << _dmaStat->even_addr_pc
      << std::dec << " " << _dmaStat->even_addr_dma
      << std::endl;
    return oss.str();
}

// ----------------------------------------------------------------------------

int WupperUpload::prepareDataRaw( int size, int pattern_id )
{
  // Create the test data bytes according to a pattern identifier
  char data[256];
  int  i;
  if( pattern_id == 1 )
    {
      // 0x55-0xAA pattern
      for( i=0; i<256; ++i )
        if( i & 1 )
          data[i] = (char) 0xAA;
        else
          data[i] = (char) 0x55;
    }
  else if( pattern_id == 2 )
    {
      // 0xFF only
      for( i=0; i<256; ++i )
        data[i] = (char) 0xFF;
    }
  else
    {
      // Incrementing pattern
      for( i=0; i<256; ++i )
        data[i] = (char) (i & 0xFF);
    }

  // Write 'size' bytes of test data to the DMA buffer
  i64 buf_i  = _writeIndex;
  int data_i = 0;
  for( i=0; i<size; ++i, ++buf_i, ++data_i )
    _dataBuffer[buf_i] = data[data_i & 0xFF];

  // DMA is started in upload()
  _bytesToUpload = size;
  _writeIndex = buf_i;
  //_uploadIndex = 0;

  return size;
}

// ----------------------------------------------------------------------------

int WupperUpload::prepareDataRaw( const std::string &filename, bool binary )
{
  std::ifstream file( filename );
  if( !file.is_open() )
    {
      _errString = "Failed to open file: ";
      _errString += filename;
      cout << "###" << this->errorString() << endl;
      return -1; // Something went wrong
    }
  cout << "Opened file " << filename << endl;

  i64 bytes_read = 0, bytes_to_upload = 0;
  i64 buf_i = _writeIndex;
  int line_nr = 0;
  if( binary )
    {
      // Storage for the number of bytes to read from the file at a time
      char data[1024*60]; // 1024 * maximum number of payload bytes
      int i, data_i;

      file.read( data, sizeof(data) );
      bytes_read = file.gcount();
      while( bytes_read > 0 )
        {
          bytes_to_upload += bytes_read;
          data_i = 0;

          for( i=0; i<bytes_read; ++i, ++buf_i, ++data_i )
            _dataBuffer[buf_i] = data[data_i];

          file.read( data, 1024*_toWupperPayloadBytes );
          bytes_read = file.gcount();
        }
    }
  else
    {
      // Text file: lines with data(word) values
      std::string line;
      std::getline( file, line );
      while( !file.eof() )
        {
          ++line_nr;

          if( line.empty() )
            {
              // Next line
              std::getline( file, line );
              continue;
            }

          // Remove leading white-space
          size_t startpos = line.find_first_not_of( " \t" );
          if( startpos != string::npos && startpos != 0 )
            line = line.substr( startpos );

          std::istringstream iss;
          unsigned int val;
          if( line[0] == '#' )
            {
              // Comment line: skip it
              std::getline( file, line );
              continue;
            }
          else
            {
              // Line with (MROD) data
              int cnt = 1; // Entry counter
              iss.str( line );
              //cout << "Line " << line_nr << ": " << line << endl;
              iss >> std::hex; // Word values: hexadecimal
              while( !iss.eof() )
                {
                  // Next value (or end-of-file/line)
                  iss >> val;
                  if( !iss.fail() )
                    {
                      unsigned char *buf =
                        (unsigned char *) &_dataBuffer[buf_i];
                      *buf = (unsigned char) ((val>> 0) & 0xFF); ++buf;
                      *buf = (unsigned char) ((val>> 8) & 0xFF); ++buf;
                      *buf = (unsigned char) ((val>>16) & 0xFF); ++buf;
                      *buf = (unsigned char) ((val>>24) & 0xFF); ++buf;
                      buf_i += 4;
                      bytes_read += 4;
                    }
                  else if( !iss.eof() )
                    {
                      std::string s;
                      iss.clear();
                      iss >> s; // Skip past the illegal entry
                      cout << "###Line " << line_nr
                           << ", illegal entry #" << cnt << endl;
                    }
                  ++cnt;
                }
            }

          if( bytes_read <= 0 )
            {
              // Nothing to upload, go to next line
              std::getline( file, line );
              bytes_read = 0;
              continue;
            }
          bytes_to_upload += bytes_read;

          // Next line
          std::getline( file, line );
          bytes_read = 0;
        }
    }

  file.close();
  cout << "Bytes read: " << bytes_to_upload
       << " (words: " << bytes_to_upload/4 << ")";
  if( !binary )
    cout << ", lines: " << line_nr;
  cout << endl;

  if( bytes_read == 0 )
    {
      // DMA is started in upload()
      _bytesToUpload = ((bytes_to_upload + 31)/32) * 32;
      _writeIndex = buf_i;

      cout << "Bytes in upload: " << _bytesToUpload << endl;

      return _bytesToUpload;
    }
  return -1; // Something went wrong
}

// ----------------------------------------------------------------------------

bool WupperUpload::upload( int speed_factor )
{
  if( _bytesToUpload == 0 )
    return false;

  if( this->dmaActive() )
    {
      //struct timespec ts;
      //ts.tv_sec  = 0;
      //ts.tv_nsec = (_toWupperBlockBytes/8) * 1000; // in nanoseconds
      //nanosleep( &ts, 0 );
      this->pause( _toWupperBlockBytes/8 ); // in microseconds
      return false;
    }

  // Timestamp the start of an upload operation (the timestamp is reset
  // to zero when read by a call to function timestamp())
  if( _timestamp == 0 )
    {
      struct timespec ts;
      clock_gettime( CLOCK_REALTIME, &ts );
      _timestamp = ts.tv_sec*1000000000 + ts.tv_nsec;
    }

  u64 dma_size;
  if( _uploadDmaSize == 0 || _bytesToUpload < _uploadDmaSize )
    dma_size = _bytesToUpload;
  else
    dma_size = _uploadDmaSize;
  if( _uploadIndex + dma_size > (u64) _bufferSize )
    // Limit DMA to up to end of buffer (in case of single-shot DMA)
    dma_size = (u64) _bufferSize - _uploadIndex;

  // Delay handling required?
  if( !_delays.empty() )
    {
      delay_t &d = _delays.front();
      if( _uploadIndex == d.index )
        {
          // Arrived at a delay index: insert the requested delay
          this->pause( d.delay_us );
          // This delay has been handled, get rid of it
          _delays.pop_front();
        }
      if( !_delays.empty() )
        {
          delay_t &d = _delays.front();
          if( _uploadIndex + dma_size > d.index )
            {
              // Upcoming delay: adjust the DMA size to arrive at the index
              // where the requested delay is to be inserted
              dma_size = d.index - _uploadIndex;
            }
        }
    }

  this->dmaStart( _uploadIndex, dma_size );

  // Update upload administration
  _bytesToUpload -= dma_size;
  _uploadIndex   += dma_size;
  if( _uploadIndex >= (u64) _bufferSize ) {
    //cout << "WRAP @ " << _uploadIndex << endl;
    _uploadIndex = 0;
  }

  // Limit overall upload rate to a maximum of 8 MB/s
  // (for a 2-bit E-link, could be twice that for a 4-bit E-link,
  //  and 4x that for an 8-bit E-link)
  if( dma_size > 127 )
    //this->pause( (dma_size/(8*speed_factor))+1 );
    this->pause( dma_size/(8*speed_factor) );

  return true;
}

// ----------------------------------------------------------------------------

bool WupperUpload::uploadFinished()
{
  return( _bytesToUpload == 0 && !this->dmaActive() );
}

// ----------------------------------------------------------------------------

bool WupperUpload::startPlayback( const std::string &filename, int blocksize )
{
  if( _pbFile.is_open() ) _pbFile.close();

  _pbFile.open( filename.c_str(), std::ifstream::binary );
  if( !_pbFile.is_open() )
    {
      _errString = "Failed to open playback file: ";
      _errString += filename;
      return false;
    }

  // Initialize playback counters
  _pbBlockCntr = 0;
  _pbChunkCntr = 0;

  // Initialize E-link intermediate buffers
  _eBuf[0].clear();

  if( blocksize > 16384 )
    {
      _errString = "Playback of blocks > 16K currently not supported";
      return false;
    }
  else if( blocksize > 0 )
    {
      // Force a different size than read from the Wupper-device itself
      _blockSize = blocksize;
      _blockSizeCode  = (_blockSize/1024-1) << 8;
    }

  // In case of random data upload
  unsigned int seed = 1;
  srandom( seed );

  return true;
}

// ----------------------------------------------------------------------------

bool WupperUpload::playback( int elink_filter, int speed_factor )
{
  // Allow up to 16K blocks (see startPlayback())
  char block[16384];//[BLOCK_BYTES];
  _pbFile.read( block, _blockSize );
  i64 bytes_read = _pbFile.gcount();
  if( bytes_read != _blockSize )
    return false;

  //_uploadIndex   = 0; // OLD method (one-shot DMAs from address 0)
  //_writeIndex    = 0; // OLD method (one-shot DMAs from address 0)
  _bytesToUpload = formatBlockForUpload( block, elink_filter );
  //this->dumpData( _bytesToUpload, _writeIndex - _bytesToUpload );
  if( _bytesToUpload > 0 )
    {
      ++_pbBlockCntr;

      while( !this->uploadFinished() )
        {
          //cout << "playback upload:" << _bytesToUpload << endl;
          this->upload( speed_factor );
        }
    }

  return true;
}

// ----------------------------------------------------------------------------

void WupperUpload::dumpData( int size, int offset )
{
  cout << "Blocksize (payload): " << _toWupperBlockBytes  << " ("
       << _toWupperPayloadBytes << ")";
  char *data = &_dataBuffer[offset];
  cout << hex << setfill('0');
  for( int i=0; i<size; ++i )
    {
      if( (i & 31) == 0 ) cout << endl << setw(6) << i+offset;
      cout << ' ' << setw(2) << ((int) data[i] & 0xFF);
    }
  cout << dec << endl;
}

// ----------------------------------------------------------------------------

std::string WupperUpload::firmwareVersion()
{
  if( _wupper == 0 ) return std::string();
  return std::string("2.0.0");
}

// ----------------------------------------------------------------------------

int WupperUpload::firmwareMode()
{
  if( _wupper == 0 ) return 0xFF;
  return 0;
}

// ----------------------------------------------------------------------------

bool WupperUpload::fullmodeType()
{
  if( _wupper == 0 ) return false;
  return 0;
}

// ----------------------------------------------------------------------------

bool WupperUpload::lpgbtType()
{
  if( _wupper == 0 ) return false;
  return 0;
}

// ----------------------------------------------------------------------------

int WupperUpload::numberOfChans()
{
  if( _wupper == 0 ) return 0;
  return 0;
}

// ----------------------------------------------------------------------------
// Private functions
// ----------------------------------------------------------------------------

char *WupperUpload::cmemAllocate( u64 buffer_size, int id )
{
  u64   cmemPhysAddr = 0;
  char *buffer = 0;

  // Open CMEM(_RCC) and allocate the requested buffer size
#ifdef USE_LIB_CMEM
  u64 cmemVirtAddr;
  int ret = CMEM_Open();
  ostringstream oss;
  oss << "WupperUpload";
  if( id >= 0 ) oss << id;
  
  if( ret == CMEM_RCC_SUCCESS )
    ret = CMEM_GFPBPASegmentAllocate( buffer_size, oss.str().c_str(), &_cmemHandle );
  if( ret == CMEM_RCC_SUCCESS )
    ret = CMEM_SegmentPhysicalAddress( _cmemHandle, &cmemPhysAddr );
  if( ret == CMEM_RCC_SUCCESS )
    ret = CMEM_SegmentVirtualAddress( _cmemHandle, &cmemVirtAddr );
  if( ret == CMEM_RCC_SUCCESS )
    {
      buffer = (char *) cmemVirtAddr;
    }
  else
    {
      rcc_error_print( stdout, ret );
      if( !_errString.empty() ) _errString += "; ";
      std::ostringstream oss;
      oss << "CMEM_RCC error: 0x" << std::hex << ret;
      _errString += oss.str();
    }

#else
  if( (_cmemHandle = open("/dev/cmem_rcc", O_RDWR)) < 0 )
    {
      if( !_errString.empty() ) _errString += "; ";
      _errString += "Failed to open /dev/cmem_rcc";
      return 0;
    }

  if( id >= 0 )
    sprintf( _cmemDescriptor.name, "WupperUpload%d", id );
  else
    sprintf( _cmemDescriptor.name, "WupperUpload" );
  _cmemDescriptor.size    = buffer_size;
  _cmemDescriptor.order   = 0;
  _cmemDescriptor.type    = TYPE_GFPBPA;
  _cmemDescriptor.numa_id = 0;
  if( CMEM_RCC_SUCCESS == ioctl(_cmemHandle, CMEM_RCC_GET, &_cmemDescriptor) )
    {
      i64 result = (i64) mmap( 0, buffer_size, PROT_READ|PROT_WRITE,
                               MAP_SHARED, _cmemHandle,
                               (i64) _cmemDescriptor.paddr );
      if( result != -1 )
        {
          _cmemDescriptor.uaddr = result;
          if( CMEM_RCC_SUCCESS == ioctl(_cmemHandle, CMEM_RCC_SETUADDR,
                                        &_cmemDescriptor) )
            buffer = (char *) _cmemDescriptor.uaddr;

          cmemPhysAddr = _cmemDescriptor.paddr;
          //cmemVirtAddr = _cmemDescriptor.uaddr;
        }
      if( buffer == 0 )
        {
          if( !_errString.empty() ) _errString += "; ";
          _errString += "mmap/ioctl(SETUADDR) failed";
        }
    }
  else
    {
      if( !_errString.empty() ) _errString += "; ";
      _errString += "ioctl(GET) failed";
    }
#endif // USE_LIB_CMEM
  if( buffer == 0 )
    {
      _cmemStartAddr = 0;
      _cmemEndAddr   = 0;
    }
  else
    {
      _cmemStartAddr = cmemPhysAddr;
      _cmemEndAddr   = _cmemStartAddr + buffer_size;
    }

  return buffer;
}

// ----------------------------------------------------------------------------

void WupperUpload::dmaStart( u64 upload_index,
                          u64 size )
{
  if( !(_wupper && _cmemStartAddr != 0 && size >= 0)  )
    return;

  if( _dmaCircular )
    {
      // Circular DMA still to be set up?
      if( (_bar0->DMA_DESC_ENABLE & (1 << _dmaIndex)) == 0 )
        {
          _wupper->dma_from_host( _dmaIndex, _cmemStartAddr, _bufferSize,
                               WUPPER_DMA_WRAPAROUND | (_dmaTlp << 16) );

          _dmaSoftwPtr = _dmaDesc->read_ptr;

          // Adjust TLP: set to its minimum of 32 bytes
          // (to guarantee even a single GBT-SCA reply gets uploaded)
          // ###SHOULDN'T DO IT AFTER DMA ENABLE: dma_from_host() should do it
          //_dmaDesc->tlp = 32 / 4;
        }

      // Update the read pointer for circular DMA
      _dmaSoftwPtr += size;
      if( _dmaSoftwPtr >= _cmemEndAddr )
        _dmaSoftwPtr -= _bufferSize;
      _dmaDesc->read_ptr = _dmaSoftwPtr;
    }
  else
    {
      // Just in case...
      this->dmaStop();

      // Configure and start a single-shot DMA
      _wupper->dma_from_host( _dmaIndex, _cmemStartAddr + upload_index, size,
                           0 | (_dmaTlp << 16) );
    }
}

// ----------------------------------------------------------------------------

void WupperUpload::dmaStop()
{
  if( _wupper == 0 ) return;

  _wupper->dma_stop( _dmaIndex );

  // Reset etc.
  //_Wupper->soft_reset();
  //usleep( 10000 );
  //_Wupper->dma_reset();
  //usleep( 10000 );
  //_Wupper->dma_fifo_flush(); OBSOLETE
  //usleep( 10000 );
}

// ----------------------------------------------------------------------------

bool WupperUpload::dmaActive()
{
  if( _wupper == 0 ) return false;

  if( _dmaCircular )
    {
      //return( _dmaStat->fw_pointer != _dmaSoftwPtr );

      // ### Temporary?: 'fw_pointer' remains 1 TLP-size behind...
      u64 fw_ptr = _dmaStat->current_address;
      if( _dmaSoftwPtr > fw_ptr )
        // If consumer (fw_ptr) is 32 bytes or less behind the producer:
        // DMA not active
        return( (_dmaSoftwPtr - fw_ptr) > 32 );
      else if( _dmaSoftwPtr < fw_ptr )
        // If consumer (fw_ptr) is somewhere at top of buffer,
        // and producer at start of buffer, DMA is still active
        return true;
      else
        return false;
    }
  else
    {
      return( (_bar0->DMA_DESC_ENABLE & (1 << _dmaIndex)) != 0 );
    }
}



void WupperUpload::pause( int delay_us )
{
  // Use usleep() when appropriate, but run a busy-wait loop for short delays
  // (determined by usleep() overhead) to achieve better timing precision
  if( delay_us > _minimumUsleep )
    {
      usleep( delay_us - _minimumUsleep );
    }
  else if( delay_us > 0 )
    {
      struct timespec ts1, ts2;
      volatile long s, t;
      clock_gettime( CLOCK_REALTIME, &ts1 );
      s = ts1.tv_sec*1000000000 + ts1.tv_nsec;
      clock_gettime( CLOCK_REALTIME, &ts2 );
      t = ((ts2.tv_sec*1000000000 + ts2.tv_nsec) - s)/1000;
      while( t < delay_us )
        {
          clock_gettime( CLOCK_REALTIME, &ts2 );
          t = ((ts2.tv_sec*1000000000 + ts2.tv_nsec) - s)/1000;
        }
    }
}

// ----------------------------------------------------------------------------

int WupperUpload::minimumUsleep()
{
  // Determine the time overhead it takes to execute usleep(1), in microseconds
  struct timespec ts1, ts2;
  clock_gettime( CLOCK_REALTIME, &ts1 );
  // Average over 4 calls..
  usleep( 1 );
  usleep( 1 );
  usleep( 1 );
  usleep( 1 );
  clock_gettime( CLOCK_REALTIME, &ts2 );
  long t = ((ts2.tv_sec*1000000000 + ts2.tv_nsec) -
            (ts1.tv_sec*1000000000 + ts1.tv_nsec))/4;
  if( t < 1000 || t > 1000000 )
    t = 50000;
  return (t-1000+500)/1000; // In microseconds
}

